#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include "sdkconfig.h"
#include "esp_log.h"


#define PRINTER_UART_NUM UART_NUM_1
#define PIN_NUM_TX GPIO_NUM_5   //A5
#define PIN_NUM_RX GPIO_NUM_4   //A6
void bsp_init_uart(){
    
     const uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };
    // Configure UART parameters
    ESP_ERROR_CHECK(uart_param_config(PRINTER_UART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(UART_NUM_1, PIN_NUM_TX, PIN_NUM_RX, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
    ESP_ERROR_CHECK(uart_driver_install(PRINTER_UART_NUM, 1024 * 2, 1024 * 2, 0, NULL, 0));

    //uint8_t *data = (uint8_t *) malloc(1024);

    // // 帧头(2) | 命令(1) | 数据长度(1) | 数据(n) | 校验位(1) | 帧尾(1)
    // AA AA       00       02          00 5A                 BB
    uint8_t test_servo[]={0xAA,0xAA,0x00,0x02,0x00,0x5a,0x00,0xBB};
    int angle = 45;
    while (1) {
        for (size_t i = 0; i < 8; i++)
        {   
            for (size_t j= 0; j < 180; j+=10){
                test_servo[4] = 0;
                test_servo[5] = j;
                uart_write_bytes(PRINTER_UART_NUM, test_servo, 8);
                printf("angle=%d\r\n",test_servo[5]);
                vTaskDelay(pdMS_TO_TICKS(10));
            }
            
        }
        //angle = (angle+10)%180;
        // Read data from the UART
        //int len = uart_read_bytes(PRINTER_UART_NUM, data, (1023 - 1), 200);
        // Write data back to the UART
        
        //if (len) {
            
            //data[len] = '\0';
            //printf("Recv str: %s\n", (char *) test_servo);
        //}
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void example_uart0_main(){
    xTaskCreate(bsp_init_uart, "uart_echo_task", 2048 , NULL, 10, NULL);
}